#!/usr/bin/env python
#coding:utf-8
import rospy
from nav_msgs.msg import OccupancyGrid #地图消息
from geometry_msgs.msg import PoseWithCovarianceStamped #位姿消息
from geometry_msgs.msg import Twist #速度消息
import time
import cv2
import numpy as np
import matplotlib.pyplot as plt
import astar
import astar2

pose = None
position = None
orientation = None
M = None
goal = [0,0]

def astar_plan(M, start, goal):
    path = []
    return path

def dwa_plan(M, path, start, goal):
    speed = 0
    turn = 0
    return speed, turn

#AMCL定位订阅回调函数
def amcl_cb(msg):
    #引入位置和姿态全局变量
    global pose, position, orientation
    #当前位姿
    pose = msg.pose.pose
    #当前位置
    position = msg.pose.pose.position
    #当前姿态
    orientation = msg.pose.pose.orientation
    print('position', position)

#地图订阅回调函数
def map_cb(msg):
    #引入地图全局变量
    global M
    if M is not None:
        return
    w = msg.info.width #地图宽度
    h = msg.info.height #地图高度
    resolution = msg.info.resolution #地图分辨率
    data = msg.data #地图数据(w*h个)
    M = [] #地图矩阵
    for i in range(h): #地图第i行
        row = []
        for j in range(w): #地图第j列
            row.append(data[i*w+j])
        M.append(row)
    M = np.array(M)

    M = M[200:401, 180:481]
    #M = M[350:551, 350:651]
    h,w = M.shape[0], M.shape[1]

    #地图降采样
    N = 5
    M_new = []
    for i in range(0,h-N,N):
        row_new = []
        for j in range(0,w-N,N):
            seg = M[i:i+N, j:j+N]
            c = np.max(seg)
            if c == -1: #障碍物灰色
                c = 128
            if c == 0:#可行驶区域白色
                c = 255
            if c == 100:#障碍物黑色
                c = 0
            row_new.append(c)
        M_new.append(row_new)
    M = M_new
    M = np.array(M)
    h,w = M.shape[0], M.shape[1]

    obs = np.zeros((h,w))
    obs[M==0] = 1
    wet = np.zeros((h,w))
    start = (30,18)
    goal = (8,45)

    graph = astar2.Graph(w,h,obs,wet)
    t0 = time.perf_counter()
    graph.run(start, goal)
    print('耗时{:.6f}s'.format(time.perf_counter()-t0))
    graph.showSeq()
    exit()


#初始化节点
rospy.init_node('astar_dwa_plan_node')
#速度发布者
pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
#地图订阅
sub = rospy.Subscriber('/map', OccupancyGrid, map_cb)
#AMCL定位订阅
sub = rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped, amcl_cb)

#循环频率为2Hz
rate = rospy.Rate(2)
while not rospy.is_shutdown():
    msg = Twist() #机器人速度
    #A*全局路径规划
    path = astar_plan(M, pose, goal)
    #DWA局部路径规划
    speed,turn = dwa_plan(M, path, pose, goal)
    #设置线速度
    msg.linear.x = speed
    #设置角速度
    msg.angular.z = turn
    #发布速度
    pub.publish(msg)
    #控制循环频率
    rate.sleep()

